//# Framework for our ARM/ATMEGA32 program.  
    
  /*  
  Copyright 2010 Anthony Oko, Koby Hitchen, Mara Cairo, Shaochen Xu, Tyler Lucas  
    
     This file is part of the trajctrl program.  
    
     trajctrl is licensed under the Apache License, Version 2.0 (the "License");  
     you may not use this file except in compliance with the License.  
     You may obtain a copy of the License at  
    
         http://www.apache.org/licenses/LICENSE-2.0  
    
     Unless required by applicable law or agreed to in writing, software  
     distributed under the License is distributed on an "AS IS" BASIS,  
     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  
     See the License for the specific language governing permissions and  
     limitations under the License.  
  */  
    
#include <stdlib.h>				// Standard C++ library
//#include <libtraj.h>			// custom library  
#include <avr/io.h>				// avr-libc pins library    
#include <stdio.h>				// avr-libc ??? library
#include <util/delay.h>			// avr-libc delay library
#include <avr/interrupt.h>		// avr-libc interrupt library

#define F_CPU 16000000UL		// external oscillator frequency

// function prototypes
void setup_adc(unsigned char qpd_stage);  
void setup_mux(unsigned char qpd_stage); 
short int *adc_data(unsigned char qpd_stage);  
short int lookup_table(unsigned char qpd_stage,short int *acd_result);
short int adj_time(unsigned char qpd_stage,short int *acd_result);  
void dac_output(short int voltage);  
void laser(void);  
    
int main(void)
{
	short int *ADCresult_ptr; 
	unsigned char qpd_stage=0;
	short int voltage=0; 
	short int adc_result[8];  
	unsigned char fire_laser='Y'; //if equal to Y then laser will fire after qpd stage #4
	
	setup_adc(qpd_stage);   
	PORTD |= (1<<PD6); //turn on crappy LED
	PORTD |= (1<<PD5); //turn on all high intensity LEDs
	sei(); //enable global interrupts
	
	//Activate solenoid(s) for injector system 
	
	//Use external interrupts (int0 or int1) to start timer when bead passes first photodiode  
	MCUCR |= (0<<ISC11)|(1<<ISC10); //logical change on int1 generates interupt request
	GICR |= (1<<INT1); //Enable external interrupt for int1
	//TIMSK |= (1<<OCIE1A); //enable CTC interrupt
	TCCR1B |= (1<<WGM12); //configure timer 1 for CTC mode 
	OCR1A = 15624; //set CTC compare A value to 1s for 16MHz clock with prescalar of 1024 
	
	for(;;) 
	{ 
		setup_adc(qpd_stage); //setup adc parameters for specific qpd stage number (1-6)  
		setup_mux(qpd_stage); //selects proper QPD to use for measurements
		
		while(!(TIFR & (1<<OCF1A))) {}; //Is timer 1A output compare flag set 
		
		TIFR = (1<<OCF1A); //clear the CTC flag for A 
		ADCresult_ptr=adc_data(qpd_stage);
		short int time_adjust=adj_time(qpd_stage,ADCresult_ptr);
		OCR1A = time_adjust; //set compare regiser A to new value
		voltage = lookup_table(qpd_stage,ADCresult_ptr);  
		
		if(qpd_stage==4 && fire_laser=='Y')  
			laser(); 
		else 
			dac_output(voltage); //apply voltage to EM plates 
	} 
	qpd_stage++; //increment stage number 
    
	return 0; 
}  
    
  // ********************* END OF MAIN ********************* //  
    
  
//Interrupt Service Requests

void setup_adc(unsigned char qpd_stage)  
{  
	if(qpd_stage==0)
	{
		DDRA &= 0x00;  //Set port A as input  
		PORTA &= 0x00;  //Set port A as tri-state (high impedance)  
		ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // Set ADC prescaler to 128 - 125KHz sample rate @ 16MHz   
		ADMUX |= (0<<REFS1)|(0<<REFS0); // Set ADC reference to AREF  
		ADMUX |= (1<<ADLAR); // Left ajdust result to allow easy 8 bit reading   
		ADMUX &= ~(0x1F);  //Clear bits 0-4 thereby selecting channel 0 of ADC 
		ADCSRA |= (1 << ADATE); //Enable auto-triggering  
		SFIOR |= (0<<ADTS2)|(0<<ADTS1)|(0<<ADTS0); // Set ADC to Free-Running Mode   
		ADCSRA |= (1 << ADEN);  // Enable ADC   
		ADCSRA |= (1 << ADIE);  // Enable ADC Interrupt   
		sei();   // Enable Global Interrupts
	}
	else if(qpd_stage<=4) 
	{ 
		DDRA &= 0x00;  //Set port A as input  
		PORTA &= 0x00;  //Set port A as tri-state (high impedance)  
		ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // Set ADC prescaler to 128 - 125KHz sample rate @ 16MHz   
		ADMUX |= (0<<REFS1)|(0<<REFS0); // Set ADC reference to AREF  
		ADMUX |= (1<<ADLAR); // Left ajdust result to allow easy 8 bit reading   
		ADMUX &= ~(0x1F);  //Clear bits 0-4 thereby selecting channel 0 of ADC 
		ADMUX |= 0x04; //Select channel 4 of ADC  
		ADCSRA |= (1 << ADATE); //Enable auto-triggering  
		SFIOR |= (0<<ADTS2)|(0<<ADTS1)|(0<<ADTS0); // Set ADC to Free-Running Mode   
		ADCSRA |= (1 << ADEN);  // Enable ADC   
		//ADCSRA |= (1 << ADIE);  // Enable ADC Interrupt   
		//sei();   // Enable Global Interrupts  
	} 
	else 
	{ 
		DDRA &= 0x00;  //Set port A as input  
		PORTA &= 0x00;  //Set port A as tri-state (high impedance)  
		ADCSRA |= (1 << ADPS2) | (0 << ADPS1) | (0 << ADPS0); // Set ADC prescaler to 16 - 1MHz sample rate @ 16MHz   
		ADMUX |= (0<<REFS1)|(0<<REFS0); // Set ADC reference to AREF  
		ADMUX |= (1<<ADLAR); // Left ajdust result to allow easy 8 bit reading   
		ADMUX &= ~(0x1F);  //Clear bits 0-4 thereby selecting channel 0 of ADC  
		ADCSRA |= (1 << ADATE); //Enable auto-triggering  
		SFIOR |= (0<<ADTS2)|(0<<ADTS1)|(0<<ADTS0); // Set ADC to Free-Running Mode   
		ADCSRA |= (1 << ADEN);  // Enable ADC   
		//ADCSRA |= (1 << ADIE);  // Enable ADC Interrupt   
		//sei();   // Enable Global Interrupts 
	}  
	
} 


void setup_mux(unsigned char qpd_stage) 
{ 
	DDRC |= (1<<PC1)|(1<<PC6)|(1<<PC7); //Set port C pins 1,6, and 7 as outputs 
	if(qpd_stage==1) 
		PORTC |= (0<<PC1)|(0<<PC6)|(1<<PC7); //Select mux channel 1 
	else if(qpd_stage==2) 
		PORTC |= (0<<PC1)|(1<<PC6)|(0<<PC7); //Select mux channel 2 
	else if(qpd_stage==3) 
		PORTC |= (0<<PC1)|(1<<PC6)|(1<<PC7); //Select mux channel 3 
	else if(qpd_stage==4) 
		PORTC |= (1<<PC1)|(0<<PC6)|(0<<PC7); //Select mux channel 4 
	else if(qpd_stage==5) 
		PORTC |= (1<<PC1)|(0<<PC6)|(1<<PC7); //Select mux channel 5 
	else 
		PORTC |= (1<<PC1)|(1<<PC6)|(0<<PC7); //Select mux channel 6 
}          
short int *adc_data(unsigned char qpd_stage)  
{     
	short int adc_result[8]; 
	
	ADCSRA |= (1 << ADSC);  // Start A2D Conversions  
	
	if(qpd_stage==5) 
	{ 
		for(short int chan=1;chan<=4;chan++)  
		{   
			while(!(ADCSRA & (1<<ADIF))){} //Check if ADIF has set (conversion complete)  
			ADCSRA |= (1<<ADIF); //Clear ADIF by setting it so new conversion can begin 
			adc_result[chan-1]= ADCH;  
		}  
	} 
	else 
	{ 
		for(short int chan=5;chan<=8;chan++)  
		{   
			while(!(ADCSRA & (1<<ADIF))){} //Check if ADIF has set (conversion complete)  
			ADCSRA |= (1<<ADIF); //Clear ADIF by setting it so new conversion can begin 
			adc_result[chan-1]= ADCH;  
		}  
	} 
	
	ADCSRA &= ~(1 << ADEN);  // Disenable ADC   
	return adc_result; 
}   

short int lookup_table(unsigned char qpd_stage,short int *acd_result)  
{  
	short int cellA=0, cellB=0, cellC=0, cellD=0; 
	short int ave_diff=0, voltage=0; 
	
	if(qpd_stage==5) 
	{ 
		cellA = *(acd_result);  //top left cell of qpd  
		cellB = *(acd_result+1);  //top right cell of qpd  
		cellC = *(acd_result+2);  //bottom left cell of qpd  
		cellD = *(acd_result+3);  //bottom right cell of qpd  
	} 
	else 
	{ 
		cellA = *(acd_result+4);  //top left cell of qpd  
		cellB = *(acd_result+5);  //top right cell of qpd  
		cellC = *(acd_result+6);  //bottom left cell of qpd  
		cellD = *(acd_result+7);  //bottom right cell of qpd  
	} 
	
	ave_diff=((cellA+cellC)-(cellB+cellD))/2; //take average difference b/w left/right sides 
	
	
	voltage = ave_diff;//do calculations here from table we still need to make  
	return voltage;  
}  

short int adj_time(unsigned char qpd_stage,short int *acd_result)  
{  
	short int cellA=0, cellB=0, cellC=0, cellD=0; 
	short int ave_diff, voltage,time_factor, vert_diff, offset; 
	
	if(qpd_stage==5) 
	{ 
		cellA = *(acd_result);  //top left cell of qpd  
		cellB = *(acd_result+1);  //top right cell of qpd  
		cellC = *(acd_result+2);  //bottom left cell of qpd  
		cellD = *(acd_result+3);  //bottom right cell of qpd  
	} 
	else 
	{ 
		cellA = *(acd_result+4);  //top left cell of qpd  
		cellB = *(acd_result+5);  //top right cell of qpd  
		cellC = *(acd_result+6);  //bottom left cell of qpd  
		cellD = *(acd_result+7);  //bottom right cell of qpd  
	} 
	
	vert_diff=((cellA+cellB)-(cellC+cellD))/2; //take average difference b/w left/right sides 
	
	
	time_factor = vert_diff+offset;//do calculations here from table we still need to make  
	return time_factor;  
}  


void dac_output(short int voltage)  
{  
	DDRB |= 0xFF; //set port B as output port  
	PORTB = voltage; //assign voltage b/w 4-4.2V to base of bjt  
}  

void laser()  
{  
	DDRB |= 0xFF; //set port B ???? as output port  
	PORTB |= (1<<PB7); //turn on laser  
}  
  /*  
   Interrupt Service Routines  
   I've put these in the main program because they won't show up anywhere in the main  
   program otherwise, even symbolically. Don't want to forget they're here!  
   (See Table 18, Reset and Interrupt Vectors, on pg. 44 of the Atmega32 datasheet.)  
   (See pp. 258-273 of the avr-libc manual, version 1.6.8, but any recent version should do.)  
   n      addr.   source                          libc-avr                        details  
   -      -----   ------                          --------                        -------  
   1      $000    RESET                           RESET                           External Pin, Power-on, Brown-out, Watchdog, and JTAG AVR  
   2      $002    INT0                            INT0_vect                       External Interrupt Request 0   
   3      $004    INT1                            INT1_vect                       External Interrupt Request 1   
   4      $006    INT2                            INT2_vect                       External Interrupt Request 2   
   5      $008    TIMER2 COMP                     TIMER2_COMP_vect        Timer/Counter2 Compare Match   
   6      $00A    TIMER2 OVF                      TIMER2_OVF_vect         Timer/Counter2 Overflow   
   7      $00C    TIMER1 CAPT                     TIMER1_COMP_vect        Timer/Counter1 Capture Event   
   8      $00E    TIMER1 COMPA            TIMER1_COMPA_vect       Timer/Counter1 Compare Match A   
   9      $010    TIMER1 COMPB            TIMER1_COMPB_vect       Timer/Counter1 Compare Match B   
   10     $012    TIMER1 OVF                      TIMER1_OVF_vect         Timer/Counter1 Overflow  
   11     $014    TIMER0 COMP                     TIMER0_COMP_vect        Timer/Counter0 Compare Match   
   12     $016    TIMER0 OVF                      TIMER0_OVF_vect         Timer/Counter0 Overflow   
   13     $018    SPI, STC                        SPI_STC_vect            Serial Transfer Complete   
   14     $01A    USART, RXC                      USART_RXC_vect          USART, Rx Complete   
   15     $01C    USART, UDRE                     USART_UDRE_vect         USART Data Register Empty   
   16     $01E    USART, TXC                      USART_TXC_vect          USART, Tx Complete   
   17     $020    ADC                                     ADC_vect                        ADC Conversion Complete   
   18     $022    EE_RDY                          EE_RDY_vect                     EEPROM Ready   
   19     $024    ANA_COMP                        ANA_COMP_vect           Analog Comparator   
   20     $026    TWI                                     TWI_vect                        Two-wire Serial Interface   
   21     $028    SPM_RDY                         SPM_RDY_vect            Store Program Memory Ready  
   */  
    
 ISR(BADISR_vect)  
{  
	//for(i=0;i<50;i++) { ledmsg(LEDMSG_SOS); }
	soft_reset();  
}  

ISR(RESET)  
{  
	soft_reset();  
}  

ISR(INT1_vect)
{
	TCCR1B |= (1<<CS12)|(1<<CS10); //Start timer at Fcpu/1024;
}